"""
增强 RC 遥控器控制，使用 RC 遥控器代替触发部分载具对象函数

date: 2025/9/13
author: SiHeng Tang
file: rc_enhance.py
copyright(c) DFSA Eastern Software
此程序不能提供任何担保 WITHOUT WARRANTY OF ANY KIND
"""
import logging
from copy import deepcopy

from async_sche import TaskWrapper, BaseTaskGen, ExitGenerationError
from mengji import Vehicle
from mod_loader import ConfigHandler
from task_element import DataEndpoint

logger = logging.getLogger(__name__)
handler = ConfigHandler(__name__)

# 这是一个给 GICG 高度定制的模块，主要支持 GICG 自身功能
DEFAULT_CONFIG = {
    "label": "rc_enhance",
    "mission_guide_label": "mg",
    "preselect_target_chan": 6,
    "select_ensure_chan": 7,
    "manual_abort_chan": 9,
    "dry_run": 1
}

MIN_CHAN_DIFF = 10


class RemoteControlEnhance(BaseTaskGen):
    """
    通过遥控器输入替代部分地面站遥测功能，主要用于测试时降低对远距离高速数传的要求
    """

    def __init__(self, veh: Vehicle, cfg: dict):
        super().__init__(priority=TaskWrapper.IDLE_PRIORITY + 10)
        self._veh = veh
        self._config = cfg

        self._is_dry_run = self._config["dry_run"]

        self._last_preselection_chan = 0
        self._last_selection_ensure_chan = 0
        self._last_manual_abort_chan = 0
        self._is_selected = False
        self._target_preselected = 0

        self.recv_raw_rc_chan = DataEndpoint()

        if self._is_dry_run:
            logger.warning("RC enhance is dry running, will not release payload!")

    def _on_gen(self):
        if not self.recv_raw_rc_chan.detect_new_in_dp():
            raise ExitGenerationError()

    async def _func(self):
        if not bool(channels := self.recv_raw_rc_chan.get_dp().get_a("payload")):
            return

        preselect_chan = channels[f"chan{self._config['preselect_target_chan']}_raw"]
        select_ensure_chan = channels[f"chan{self._config['select_ensure_chan']}_raw"]
        manual_abort_chan = channels[f"chan{self._config['manual_abort_chan']}_raw"]

        if (abs(preselect_chan - self._last_preselection_chan) < MIN_CHAN_DIFF) and (
                abs(select_ensure_chan - self._last_selection_ensure_chan) < MIN_CHAN_DIFF) and (
                abs(manual_abort_chan - self._last_manual_abort_chan) < MIN_CHAN_DIFF):
            return

        self._last_preselection_chan = preselect_chan
        self._last_selection_ensure_chan = select_ensure_chan
        self._last_manual_abort_chan = manual_abort_chan

        if self._last_manual_abort_chan > 1500:
            logger.warning("manual abort triggered")
            if not self._is_dry_run:
                self._veh.get_a(self._config["mission_guide_label"]).manual_interrupt()

        # 固定三段开关映射到前三个目标序号
        if self._last_preselection_chan < 1333:
            logger.warning("preselected target >0<")
            self._target_preselected = 0
        elif self._last_preselection_chan < 1666:
            logger.warning("preselected target >1<")
            self._target_preselected = 1
        elif self._last_preselection_chan < 65534:
            logger.warning("preselected target >2<")
            self._target_preselected = 2

        if (not self._is_selected) and self._last_selection_ensure_chan > 1500:
            logger.warning(f"select target at index >{self._target_preselected}<")
            if not self._is_dry_run:
                self._veh.get_a(self._config["mission_guide_label"]).set_target_position(self._target_preselected)

    def _on_exit(self):
        self._veh = None


@handler.add_handler("virtual_bus:rc_enhance")
def load_config(veh: Vehicle, cfg_section: dict):
    logger.debug(f"RC enhance load config {cfg_section}")

    cfg = deepcopy(DEFAULT_CONFIG)
    cfg.update(cfg_section)

    veh.reg_part(cfg["label"], RemoteControlEnhance(veh, cfg), is_sche=True)
